/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "modules/planning/planning_component.h"

#include "cyber/common/file.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/util/message_util.h"
#include "modules/common/util/util.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/pnc_map/pnc_map.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/navi_planning.h"
#include "modules/planning/on_lane_planning.h"

// routing的头文件
#include "modules/routing/routing_component.h"

using apollo::common::time::Clock;
using apollo::hdmap::HDMapUtil;
using apollo::perception::TrafficLightDetection;
using apollo::relative_map::MapMsg;
using apollo::routing::RoutingRequest;
using apollo::routing::RoutingResponse;

int main()
{
  /////////////////////////////////////////////////////////////
  // routing模块

  // Load the routing request file  // routing request waypoints
  constexpr char routing_test_waypoints[] = "../src/routing_test_waypoints.txt";
  apollo::routing::RoutingRequest routing_request;
  if (!apollo::cyber::common::GetProtoFromFile(routing_test_waypoints,
                                               &routing_request))
  {
    AERROR << "failed to load file: " << routing_test_waypoints;
    return -1;
  }
  AERROR << "&routing_request   " << &routing_request;

  apollo::routing::RoutingRequest *routing_request_1 = new apollo::routing::RoutingRequest(routing_request);
  std::shared_ptr<apollo::routing::RoutingRequest> routing_request_2(routing_request_1);

  // generate topo_graph     // modules/common/data/global_flagfile.txt
  apollo::routing::Routing routing_;
  routing_.Init();
  routing_.Start();

  // routing process
  auto response = std::make_shared<apollo::routing::RoutingResponse>();
  routing_.Process(routing_request_2, response.get());
  // 打印routing的结果
  AERROR << "routing results:     " << (response->status().msg());
  response->road().size();

  for (int i = 0; i < response->road().size(); i++)
  {
    std::cout << "  " << response->road()[i].id() << std::endl;
  }

  std::cout << "End of routing ////////////////////////////////////////////////////////" << std::endl;
  // routing模块， 结束

  ////////////////////////////////////////////////////////////////
  // planning模块
  apollo::planning::PlanningComponent planning_component_;
  printf("planning_demo  %d \n", __LINE__);
  planning_component_.Init();

  // // 要求输入的数据
  // std::shared_ptr<apollo::prediction::PredictionObstacles> prediction_obstacles;
  // std::shared_ptr<apollo::canbus::Chassis> chassis;
  // std::shared_ptr<apollo::localization::LocalizationEstimate> localization_estimate;

  // planning_component_.Proc(prediction_obstacles, chassis, localization_estimate);
  //   apollo::planning::NaviPlanning planning_base_demo;

  //   std::unique_ptr<apollo::planning::PlanningBase>
  //       planning_base_ = std::make_unique<apollo::planning::NaviPlanning>();

  //   printf("  %d \n", __LINE__);
  //   apollo::planning::PlanningConfig config_;
  //   CHECK(apollo::cyber::common::GetProtoFromFile(FLAGS_planning_config_file,
  //                                                 &config_))
  //       << "failed to load planning config file " << FLAGS_planning_config_file;
  //   printf("  %d \n", __LINE__);

  //   planning_base_->Init(config_);
  //   printf("  %d \n", __LINE__);

  // // cyber
  // std::shared_ptr<apollo::cyber::Node> node_ = nullptr;

  // std::shared_ptr<apollo::cyber::Reader<apollo::routing::RoutingResponse>> routing_reader_;
  //   routing_reader_ = node_->CreateReader<apollo::routing::RoutingResponse>(
  //       FLAGS_routing_response_topic,
  //       [this](const std::shared_ptr<apollo::routing::RoutingResponse>& routing) {
  //         AINFO << "Received routing data: run routing callback."
  //               << routing->header().DebugString();
  //         std::lock_guard<std::mutex> lock(mutex_);
  //         routing_.CopyFrom(*routing);
  //       });

  //   printf("  %d \n", __LINE__);

  //   printf("  %d \n", __LINE__);

  // planning模块， 结束
}
